#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/*
    订阅方实现：订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换

    包含头文件
    编码　初始化　NodeHandle
    创建订阅对象　　订阅坐标系相对关系
    组织一个坐标点数据
    转换算法，需要调用ＴＦ内置实现
    最后输出
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer;//缓存订阅到的数据
    tf2_ros::TransformListener listener(buffer);//创建订阅对象
    //组织一个坐标点数据
    geometry_msgs::PointStamped ps;
    ps.header.stamp = ros::Time::now();
    ps.header.frame_id = "laser";
    ps.point.x=2.0; 
    ps.point.y=3.0; 
    ps.point.z=5.0;
    //添加休眠
    // ros::Duration(2).sleep();
    //转换算法
    ros::Rate rate(10);
    while (ros::ok())
    {
        //核心代码实现  将ｐｓ转换成相对于base_link的坐标点
        geometry_msgs::PointStamped ps_out;
        try
        {
            ps_out = buffer.transform(ps,"base_link");

        //输出
            ROS_INFO("转换后的坐标值：(%.2f,%.2f,%.2f),参考的坐标系：%s",
                                ps_out.point.x,
                                ps_out.point.y,
                                ps_out.point.z,
                                ps_out.header.frame_id.c_str());
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%s",e.what());
        }
        


        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}
